#include "ros/ros.h"
#include "saint_nr_9/IRdata.h"
#include "roboard_drivers/adc_val.h"
#include <std_msgs/Int32.h>
#include <cmath>
#include <iostream>
#include <deque>
#include <cstdio>

#define HISTORY_LENGTH 3
#define BUMPER_PRESSED 1200
//#define CALIBRATE      
ros::Publisher pubCMIR;
struct IRSensor {
  float a;
  float b;
  float c;
  float d;
};
//red sensor calibrated (according to method1)

IRSensor red = {66.87 ,-0.00587, -0.0001523,0.01141};//{-0.001847, 0.01237, 54.25,-0.009356};

//not calibrated
IRSensor blue = {0,0,0,0};
//black is short range, calibrated
IRSensor black = {66.87 ,-0.00587, -0.0001523,0.01141};//{-0.6579,0.003213, 60.58,-0.01096};
//green short
IRSensor green = {66.87 ,-0.00587, -0.0001523,0.01141};//{-0.5312,0.003541, 64.05,-0.01163};

//yellow sensor calibrated (according to method1), calibrated
//IRSensor yellow = {-0.001847, 0.01237,  54.25,  -0.009356 };
IRSensor yellow = {66.87 ,-0.00587, -0.0001523,0.01141};//{-0.6579,0.003213, 60.58,-0.01096};

//calibrated
IRSensor white= {66.87 ,-0.00587, -0.0001523,0.01141};//{-0.7554,0.002946, 54.45, -0.01005};

//calibrated
IRSensor orange= {79.08, -0.006175, -0.002198, 0.008208};

std::deque<int> measurement_hist[8];

float calculateExpValue(IRSensor s, float val)
{
  return s.a*exp(s.b*val)+s.c*exp(s.d*val);
}

float calculateMean(std::deque<int> q) {

  float sum = 0;
  for (std::deque<int>::iterator it(q.begin()); it != q.end(); ++it) {
    sum += *it;
  }
  
  return (sum/q.size());

  /*std::deque<int> copy = q;
  std::sort(copy.begin(), copy.end());
  return copy[copy.size()/2];*/
}

void IRRawCallback(const roboard_drivers::adc_val::ConstPtr& data)
{
  saint_nr_9::IRdata d;
  d.dt = data->timestamp;
  measurement_hist[0].push_back(data->val0);
  measurement_hist[1].push_back(data->val1);
  measurement_hist[2].push_back(data->val2);
  measurement_hist[3].push_back(data->val3);
  measurement_hist[4].push_back(data->val4);
  measurement_hist[5].push_back(data->val5);
  measurement_hist[6].push_back(data->val6);
  measurement_hist[7].push_back(data->val7);

  for (int i = 0; i < 8; i++) {
    if (measurement_hist[i].size() > HISTORY_LENGTH) {
      measurement_hist[i].pop_front();
    }
  }
  
  #ifdef CALIBRATE 
  if (measurement_hist[0].size() == HISTORY_LENGTH) {
    printf("0: %f2.2 \t 1(R): %f2.2 \t 2(Y): %f2.2 \t 3(G): %f2.2 4(W): %f2.2 \t 5(Bla): %f2.2 \t 6(O): %f2.2 \t 7(Blu): %f2.2 \n",
	     calculateMean(measurement_hist[0]),
	     calculateMean(measurement_hist[1]),
	     calculateMean(measurement_hist[2]),
	     calculateMean(measurement_hist[3]),
	     calculateMean(measurement_hist[4]),
	     calculateMean(measurement_hist[5]),
	     calculateMean(measurement_hist[6]),
	     calculateMean(measurement_hist[7]));
  }
  #endif
  if (measurement_hist[0].size() < HISTORY_LENGTH) return;

  d.red = calculateExpValue(red, calculateMean(measurement_hist[1]))-2;
  d.yellow = calculateExpValue(yellow, calculateMean(measurement_hist[2]))-2;
  d.green = calculateExpValue(green, calculateMean(measurement_hist[3]));
  //d.blue = calculateExpValue(blue, calculateMean(measurement_hist[7]));
  d.black = calculateExpValue(black,calculateMean(measurement_hist[5]));
  d.orange = calculateExpValue(orange, calculateMean(measurement_hist[6]));
  d.white = calculateExpValue(white, calculateMean(measurement_hist[4]));
  if (data->val7 > BUMPER_PRESSED)
  {
    d.blue = 1;
  }
  else
  {
    d.blue = 0;
  }
  pubCMIR.publish(d);
}

int main(int argc, char**argv) {
  ros::init(argc, argv, "listenIR");
  ros::NodeHandle n;
  pubCMIR = n.advertise<saint_nr_9::IRdata>("ir_info", 1);
  
  ros::Subscriber sub =  n.subscribe("/roboard/adc", 1, IRRawCallback);
  ros::Publisher chatter_pub = n.advertise<std_msgs::Int32>("roboard/adc_interval", 1000);
  std_msgs::Int32 msg;		
  msg.data = 200;
  chatter_pub.publish(msg);
  ros::spin();
  
}
